//
// Created by jg on 2021/1/4.
//


#include "fast_marching/fast_marching.h"

void Path_FMM::init(ros::NodeHandle &nh) {
    nh.param("planning/max_vel",       MAX_VEL_,  -1.0);
    nh.param("planning/max_acc",       MAX_ACC_,  -1.0);
}

void Path_FMM::setFMMenv(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt){
    start_pt_ = std::move(start_pt);
    end_pt_   = std::move(end_pt);

    map_edt_->getFMMGrid(grid_fmm_, max_vel_);
    auto tmp = map_edt_->FMMSet2Point(grid_fmm_, start_pt_, end_pt_);
    startIndices_ = tmp.first;
    goalIdx_      = tmp.second;

    fm_solver_->clear();
    fm_solver_->setEnvironment(&grid_fmm_);
    fm_solver_->setInitialAndGoalPoints(startIndices_, goalIdx_);
}

void Path_FMM::getfMMPath(vector<Eigen::Vector3d> &path_coord, vector<double>& time) {
    if(max_vel_ == -1){
        ROS_WARN("eht environment of Fast Marhcing Methods is not ready!");
        return;
    }

    ros::Time time_bef_fm = ros::Time::now();
    if (fm_solver_->compute(max_vel_) == -1){
        ROS_WARN("[Fast Marching Node] No path can be found");
        return;
    }
    ros::Time time_aft_fm = ros::Time::now();
    ROS_INFO("[Fast Marching Node] Time in Fast Marching computing is %f", (time_aft_fm - time_bef_fm).toSec() );

    Path3D path3D;
    vector<double> path_vels;
    GradientDescent< FMGrid3D > grad3D;

    if (grad3D.gradient_descent(grid_fmm_, goalIdx_, path3D, path_vels, time) == -1) {
        ROS_WARN("[Fast Marching Node] FMM failed, valid path not exists");
        return;
    }

    path_coord.push_back(start_pt_);
    map_edt_->FMMPath3D2vecPath(path3D, path_coord);

    path_ = path_coord;
}

void Path_FMM::getCorridor(vector<Cube> &corridor, vector<double> &time, Eigen::Vector3d vel) {
    start_vel_ = std::move(vel);

    ros::Time time_bef_corridor = ros::Time::now();
    this->sortPath(path_, time);
    map_edt_->getCorridor(corridor, path_, time);
    ros::Time time_aft_corridor = ros::Time::now();
    ROS_WARN("Time consume in corridor generation is %f", (time_aft_corridor - time_bef_corridor).toSec());

    if(corridor.empty()){
        ROS_WARN("[FMM]: get corridor fail !");
        return;
    }

    this->timeAllocation(corridor, time);
}

void Path_FMM::sortPath(vector<Eigen::Vector3d>& path_coord, vector<double>& time) {
    vector<Eigen::Vector3d> path_tmp;
    vector<double> time_tmp;

    for (int i = 0; i < (int)path_coord.size(); i += 1)
    {
        if( i )
            if( std::isinf(time[i]) || time[i] == 0.0 || time[i] == time[i-1] )
                continue;

        if( (path_coord[i] - end_pt_).norm() < 0.2)
            break;

        path_tmp.push_back(path_[i]);
        time_tmp.push_back(time[i]);
    }
    path_coord.swap(path_tmp);
    time.swap(time_tmp);
}

void Path_FMM::timeAllocation(vector<Cube> &corridor, vector<double> &time) {
    vector<double> tmp_time;

    for(int i  = 0; i < (int)corridor.size() - 1; i++)
    {
        double duration  = (corridor[i].t - corridor[i+1].t);
        tmp_time.push_back(duration);
    }
    double lst_time = corridor.back().t;
    tmp_time.push_back(lst_time);

    vector<Eigen::Vector3d> points;
    points.push_back(start_pt_);
    for(int i = 1; i < (int)corridor.size(); i++)
        points.push_back(corridor[i].center);

    points.push_back(end_pt_);

    const double Expect_Vel = MAX_VEL_ * 0.7;
    const double Expect_Acc = MAX_ACC_ * 0.6;
    double _vel = Expect_Vel, _acc = Expect_Acc;

    Eigen::Vector3d initv = start_vel_;
    for(int i = 0; i < (int)points.size() - 1; i++)
    {
        double dtxyz;

        Eigen::Vector3d p0   = points[i];
        Eigen::Vector3d p1   = points[i + 1];
        Eigen::Vector3d d    = p1 - p0;
        Eigen::Vector3d v0(0.0, 0.0, 0.0);

        if( i == 0) v0 = initv;

        double D    = d.norm();    //求范数，即长度
        //velocity vector maps on distance vector
        double V0   = v0.dot(d / D);   //点乘，即速度在位移方向的投影
        double aV0  = fabs(V0);

        double acct = (_vel - V0) / _acc * ((_vel > V0)?1:-1);   // 加(减)速至预期速度_Vel时间
        double accd = V0 * acct + (_acc * acct * acct / 2) * ((_vel > V0)?1:-1);  // 加速距离
        double dcct = _vel / _acc;  //从预期速度_Vel减速至0所需时间
        double dccd = _acc * dcct * dcct / 2;  //减速距离

        if (D < aV0 * aV0 / (2 * _acc))     //speed down
        {
            double t1 = (V0 < 0)?2.0 * aV0 / _acc:0.0;
            double t2 = aV0 / _acc;
            dtxyz     = t1 + t2;
        }
        else if (D < accd + dccd)       //speed up, and then speed down
        {
            double t1 = (V0 < 0)?2.0 * aV0 / _acc:0.0;
            double t2 = (-aV0 + sqrt(aV0 * aV0 + _acc * D - aV0 * aV0 / 2)) / _acc;
            double t3 = (aV0 + _acc * t2) / _acc;
            dtxyz     = t1 + t2 + t3;
        }
        else        //speed up, then uniform speed, speed down
        {
            double t1 = acct;
            double t2 = (D - accd - dccd) / _vel;
            double t3 = dcct;
            dtxyz     = t1 + t2 + t3;
        }

        if(dtxyz < tmp_time[i] * 0.5 || dtxyz * 0.8 > tmp_time[i]) {
            tmp_time[i] = dtxyz; // if FM given time in this segment is rediculous long, use the new value
        }
        if(i == 0 || i == (int)points.size() - 1 - 1){
            tmp_time[i] = dtxyz * 2;
        }
    }

    for(int i = 0; i < (int)corridor.size(); i++){
        corridor[i].t = tmp_time[i];
    }
}
